/**
 * @author Yunfeng Bai
 * 
 * 2010-11-11
 */
package geometry;

import geometry.TriangleMesh.TriangleIndex;

import javax.media.opengl.GL2;
import javax.vecmath.Color3f;
import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import util.Util;
import collision.BoundingBox;
import collision.BoundingSphere;

public class Triangle extends Drawable
{
	// A, B, C are in counterclockwise order.
    private Point3d _a = new Point3d(), _b = new Point3d(), _c = new Point3d();
    private Vector3d _n = new Vector3d();
    private Point3d center = new Point3d();
    
    // The idea of pseudonormal is from
    // "Generating Signed Distance Fields From Triangle Meshes"
    // Pseudonormal on edges.
    private Vector3d _nab = new Vector3d(), _nbc = new Vector3d(), _nac = new Vector3d();
    // Pseudonormal on vertices.
    private Vector3d _na = new Vector3d(), _nb = new Vector3d(), _nc = new Vector3d();
    
    // Temporary variables.
    private static Point3d projectionPoint = new Point3d();
    private static Point3d tp1 =  new Point3d(), tp2 = new Point3d(), tp3 = new Point3d();
    private static Vector3d d = new Vector3d(), ap = new Vector3d();
    
    private TriangleIndex triangleIndex;
    
    public Triangle() {}

    public Triangle(Point3d a_, Point3d b_, Point3d c_, Color3f color_, TriangleIndex triangleIndex)
    {
        Vector3d n = Util.cross(Util.sub(b_, a_), Util.sub(c_, b_));
        n.normalize();
        init(a_, b_, c_, color_, n, null, null, null, null, null, null, triangleIndex);
    }
    
    public Triangle(Point3d a_, Point3d b_, Point3d c_, Color3f color_, Vector3d n_,
    		TriangleIndex triangleIndex)
    {
    	init(a_, b_, c_, color_, n_, null, null, null, null, null, null, triangleIndex);
    }
    
    public Triangle(Point3d a_, Point3d b_, Point3d c_, Color3f color_, Vector3d n_,
		    		Vector3d nab_, Vector3d nbc_, Vector3d nac_,
					Vector3d na_, Vector3d nb_, Vector3d nc_,
					TriangleIndex triangleIndex)
    {
    	init(a_, b_, c_, color_, n_, nab_, nbc_, nac_, na_, nb_, nc_, triangleIndex);
    }
    
    public void init(Point3d a_, Point3d b_, Point3d c_, Color3f color_, Vector3d n_,
    				Vector3d nab_, Vector3d nbc_, Vector3d nac_,
    				Vector3d na_, Vector3d nb_, Vector3d nc_,
    				TriangleIndex triangleIndex)
    {
    	this.triangleIndex = triangleIndex;
    	_color = new Color3f(color_);
        _a = new Point3d(a_);
        _b = new Point3d(b_);
        _c = new Point3d(c_);
        _n = new Vector3d(n_);
        _n.normalize();
        center.x = (_a.x + _b.x + _c.x) / 3;
        center.y = (_a.y + _b.y + _c.y) / 3;
        center.z = (_a.z + _b.z + _c.z) / 3;
        
        if (nab_ != null)
        {
	        _nab = new Vector3d(nab_);
	        _nac = new Vector3d(nac_);
	        _nbc = new Vector3d(nbc_);
	        _na = new Vector3d(na_);
	        _nb = new Vector3d(nb_);
	        _nc = new Vector3d(nc_);
        }
    }
    
    public Point3d getP(int id)
    {
    	switch (id)
    	{
	    	case 0: return _a;
	    	case 1: return _b;
	    	case 2: return _c;
    	}
    	return null;
    }

    public Point3d getA()
    {
        return _a;
    }

    public Point3d getB()
    {
        return _b;
    }

    public Point3d getC()
    {
        return _c;
    }
    
    public Vector3d getNormal()
    {
    	return _n;
    }
    
    public Point3d getCenter()
    {
    	return center;
    }

    public void draw(GL2 gl)
    {
        gl.glColor3f(_color.x, _color.y, _color.z);
        gl.glBegin(GL2.GL_TRIANGLES);
        gl.glNormal3d(_n.x, _n.y, _n.z);
        gl.glVertex3d(_a.x, _a.y, _a.z);
        gl.glVertex3d(_b.x, _b.y, _b.z);
        gl.glVertex3d(_c.x, _c.y, _c.z);
        gl.glEnd();
    }

    @Override
    public void build(GL2 gl)
    {
        throw new RuntimeException("should not use this method!");
    }

    /**
     * Calculate if a ray intersects a triangle. The points of the triangle
     * should be in counterclockwise order.
     * 
     * @param r
     *            The original of the ray.
     * @param d
     *            The direction of the ray.
     * @return The parameter of the ray. If it is less than 0, then no
     *         intersection.
     */
    public double intersectWithRay(Point3d r, Vector3d d)
    {
        double down = d.x * _n.x + d.y * _n.y + d.z * _n.z;
        // If the line is parallel (or overlapping) to the plane,
        // then we think there is no intersection.
        if (down == 0)
        {
            return -1;
        }
        double t = ((_a.x - r.x) * _n.x + (_a.y - r.y) * _n.y + (_a.z - r.z)
                * _n.z)
                / down;
        if (t < 0)
            return -1;
        Point3d p = Util.add(r, Util.scale(d, t));
        if (isInside(p))
            return t;
        else
            return -1;
    }
    
    public boolean isInside(Point3d p)
    {
    	Vector3d sa = Util.cross(Util.sub(_b, _a), Util.sub(p, _a));
        Vector3d sb = Util.cross(Util.sub(_c, _b), Util.sub(p, _b));
        Vector3d sc = Util.cross(Util.sub(_a, _c), Util.sub(p, _c));
        double ca = sa.dot(_n);
        double cb = sb.dot(_n);
        double cc = sc.dot(_n);
        if (ca > 0 && cb > 0 && cc > 0) return true;
        else return false;
    }

    public Point3d intersectWithSegment(Point3d a, Point3d b)
    {
    	Point3d r = new Point3d();
    	Vector3d d = new Vector3d();
        r.set(a);
        d.set(b);
        d.sub(a);
        double t = intersectWithRay(r, d);
        if (t >= 0 && t <= 1)
        {
        	Point3d p = new Point3d();
        	p.scaleAdd(t, d, r);
        	return p;
        }
        else return null;
    }

    public Point3d intersectWithTriangle(Triangle triangle)
    {
        Point3d p = null;
        p = triangle.intersectWithSegment(_a, _b);
        if (p != null)
        	return p;
        p = triangle.intersectWithSegment(_b, _c);
        if (p != null)
        	return p;
        p = triangle.intersectWithSegment(_c, _a);
        if (p != null)
        	return p;
        
        return null;
    }
    
    public boolean hasIntersectionWithTriangle(Triangle triangle)
    {
    	return ((triangle.intersectWithSegment(_a, _b) != null)
	    	 || (triangle.intersectWithSegment(_b, _c) != null)
	    	 || (triangle.intersectWithSegment(_c, _a) != null));
    }
    
    /**
     * @param transformMatrix Transform @param triangle to the frame of this triangle.
     */
    public boolean hasIntersectionWithTriangle(Triangle triangle, Matrix4d transformMatrix)
    {
    	Point3d a, b, c;
    	a = new Point3d(triangle.getA());
    	b = new Point3d(triangle.getB());
    	c = new Point3d(triangle.getC());
    			
    	transformMatrix.transform(a);
    	transformMatrix.transform(b);
    	transformMatrix.transform(c);
    	return ((intersectWithSegment(a, b) != null)
	    	 || (intersectWithSegment(b, c) != null)
	    	 || (intersectWithSegment(c, a) != null));
    }
    
    public static class ClosestPointInfo
    {
    	public int type;
    	public double distanceSqr;
    	public Point3d targetPoint = new Point3d();
    	public Triangle triangle;
    }
    
    /**
     * Get the closest point to p on the line (defined by triangle vertices indexed by ia and ib).
     * @return
     * type:
     * ia: targetPoint lies on a.
     * ib: targetPoint lies on b.
     * 3: targetPoint lies inside the line; 
     */
    private ClosestPointInfo getClosestPointOnLine(int ia, int ib, Point3d p)
    {
    	Point3d a = getP(ia);
    	Point3d b = getP(ib);
    	d.sub(b, a);
    	ap.sub(p, a);
    	// t = ((p - a) dot d) / (d dot d)
    	double t = (d.x * ap.x + d.y * ap.y + d.z * ap.z)
    			 / (d.x * d.x + d.y * d.y + d.z * d.z);
    	
    	ClosestPointInfo info = new ClosestPointInfo();
    	if (t <= 0)
    	{
    		info.targetPoint.set(a);
    		info.distanceSqr = Util.distanceSqr(p, a);
    		info.type = ia;
    	}
    	else if (t >= 1)
    	{
    		info.targetPoint.set(b);
    		info.distanceSqr = Util.distanceSqr(p, b);
    		info.type = ib;
    	}
    	else
    	{
    		info.targetPoint.scaleAdd(t, d, a);
    		info.distanceSqr = Util.distanceSqr(info.targetPoint, p);
    		info.type = 3;
    	}
    	return info;
    }
    
    /**
     * Get the closest point to p on the triangle.
     * @return
     * type:
     * 0: targetPoint lies inside the triangle;
     * 1, 2, 3: targetPoint lies on the edges of the triangle (ab, bc, ac);
     * 4, 5, 6: targetPoint lies on the vertices of the triangle (a, b, c).
     */
    public ClosestPointInfo getClosestPoint(Point3d p)
    {
    	double d = _n.x * (p.x - _a.x) + _n.y * (p.y - _a.y) + _n.z * (p.z - _a.z);
    	projectionPoint.scaleAdd(-d, _n, p);
    	
    	ClosestPointInfo info = new ClosestPointInfo();
    	if (isInside(projectionPoint))
    	{
    		info.targetPoint.set(projectionPoint);
    		info.type = 0;
    		info.distanceSqr = d * d;
    		return info;
    	}
    	else
    	{
    		// TODO optimize according to the region in which the projection point falls into.
    		ClosestPointInfo t1 = getClosestPointOnLine(0, 1, projectionPoint);
    		ClosestPointInfo t2 = getClosestPointOnLine(1, 2, projectionPoint);
    		ClosestPointInfo t3 = getClosestPointOnLine(0, 2, projectionPoint);
    		double t = Util.min(t1.distanceSqr, t2.distanceSqr, t3.distanceSqr);
    		info.distanceSqr = t + d * d;
    		if (t == t1.distanceSqr)
    		{
    			info.targetPoint = t1.targetPoint;
    			if (t1.type < 3) // Closest point on vertices.
    				info.type = t1.type + 4;
    			else // Closest point inside edge.
    				info.type = 1;
    		}
    		else if (t == t2.distanceSqr)
    		{
    			info.targetPoint = t2.targetPoint;
    			if (t2.type < 3) // Closest point on vertices.
    				info.type = t2.type + 4;
    			else // Closest point inside edge.
    				info.type = 2;
    		}
    		else if (t == t3.distanceSqr)
    		{
    			info.targetPoint = t3.targetPoint;
    			if (t3.type < 3) // Closest point on vertices.
    				info.type = t3.type + 4;
    			else // Closest point inside edge.
    				info.type = 3;
    		}
    	}
    	return info;
    }
    
    public Vector3d getNormalOnEdges(int id)
    {
    	switch (id)
    	{
	    	case 1:
	    		return _nab;
	    	case 2:
	    		return _nbc;
	    	case 3:
	    		return _nac;
    	}
    	return null;
    }
    
    public Vector3d getNormalOnVertices(int id)
    {
    	switch (id)
    	{
	    	case 4:
	    		return _na;
	    	case 5:
	    		return _nb;
	    	case 6:
	    		return _nc;
    	}
    	return null;	
    }
    
    public Triangle clone()
    {
    	Triangle tri = new Triangle(new Point3d(_a), new Point3d(_b), new Point3d(_c),
    								new Color3f(_color), new Vector3d(_n), triangleIndex);
    	return tri;
    }
    
    public void set(Triangle t)
    {
    	_a.set(t._a);
    	_b.set(t._b);
    	_c.set(t._c);
    	_n.set(t._n);
    	super.set(t);
    }
    
    public TriangleIndex getTriangleIndex()
    {
    	return triangleIndex;
    }

    @Override
    public TriangleMesh getTriangleMesh()
    {
        throw new RuntimeException("this method should not be used");
    }

    @Override
    public BoundingBox getBoundingBox()
    {
        throw new RuntimeException("Please do not use this method!");
    }
}
